#include "roboMoMo.h"

#include <stdlib.h>
#include <stdio.h>
#include <sys/types.h>
#include <string.h>
#include <errno.h>
#include <pthread.h>

/////////////////////////////////////// Config /////////////////////////////////////

static struct robo_dev *currentConfig;
static size_t conf_size;

int setConfig(struct robo_dev *conf, size_t members)
{
	int i;

	if(currentConfig) {
		for(i = 0; i < conf_size; i++) {
			free(&currentConfig[i].value_file);
		}
		currentConfig = realloc(currentConfig, members * sizeof(struct robo_dev));
	}
	else
		currentConfig = malloc(members * sizeof(struct robo_dev));

	for(i = 0; i < members; i++) {
		currentConfig[i].value_file = malloc(strlen(conf[i].value_file) + 1);

		strcpy(currentConfig[i].value_file, conf[i].value_file);
		currentConfig[i].type = conf[i].type;
		currentConfig[i].val1 = 0;
		currentConfig[i].val2 = 0;
	}
	conf_size = members;
	return 0;
}

/////////////////////////////////////// Motor /////////////////////////////////////

void setMotorValue(unsigned int confNum, int value)
{
	if(confNum > conf_size)
		return;
	struct robo_dev motor = currentConfig[confNum];

	value = (value > 255) ? 255 : value;
	value = (value < -255) ? -255 : value;

	if((currentConfig == 0 || motor.type != motor_type || motor.val1 == value) && value != 0)
		return;

	motor.val1 = value;

	FILE *fp = fopen(motor.value_file, "w");
	fprintf(fp, "%d\n", value);
	fclose(fp);
}

int getMotorValue(unsigned int confNum)
{
	if(confNum > conf_size)
		return 0;

	struct robo_dev motor = currentConfig[confNum];

	if(currentConfig == 0 || motor.type != motor_type)
		return 0;

	FILE *fp = fopen(motor.value_file, "r");
	fscanf(fp, "%d\n", &motor.val1);
	fclose(fp);

	return motor.val1;
}

//////////////////////////////////////// Servo ////////////////////////////////////////

void setServoValue(unsigned int confNum, int value)
{
	if(confNum > conf_size)
		return;

	struct robo_dev servo = currentConfig[confNum];

	value = (value > 255) ? 255 : value;
	value = (value < 0) ? 0 : value;

	if(currentConfig == 0 || servo.type != servo_type || servo.val1 == value)
		return;

	servo.val1 = value;

	FILE *fp = fopen(servo.value_file, "w");
	fprintf(fp, "%d\n", value);
	fclose(fp);
}

void disableServo(unsigned int confNum)
{
	if(confNum > conf_size)
		return;

	struct robo_dev servo = currentConfig[confNum];

	if(currentConfig == 0 || servo.type != servo_type)
		return;

	FILE *fp = fopen(servo.value_file, "w");
	fprintf(fp, "-1\n");
	fclose(fp);
}

int getServoValue(unsigned int confNum)
{
	if(confNum > conf_size)
		return 0;

	struct robo_dev servo = currentConfig[confNum];

	if(currentConfig == 0 || servo.type != servo_type)
		return 0;

	FILE *fp = fopen(servo.value_file, "r");
	fscanf(fp, "%d\n", &servo.val1);
	fclose(fp);

	return servo.val1;
}

///////////////////////////////////////// Timer ///////////////////////////////////////

struct timer_struct {
	int durration;
	int confNum;
	void (*handler)(int);
};

static void* g_start_timer(void *t)
{
	struct timer_struct *ts = (struct timer_struct*)t;

	usleep(1000 * ts->durration);
	ts->handler(ts->confNum);
	free(t);
	pthread_exit(0);
}

pthread_t after_ms_do(int durration, void (*handler)(int), int confNum)
{
	pthread_t ret;

	struct timer_struct *t = malloc(sizeof(struct timer_struct));
	t->durration = durration;
	t->handler = handler;
	t->confNum = confNum;

	if(pthread_create(&ret, 0, &g_start_timer, (void*)t))
		return -1;

	return ret;
}

//////////////////////////////////////// Buzzer ///////////////////////////////////////

static void stop_beep(int confNum)
{
	if(confNum > conf_size)
		return;

	struct robo_dev buzzer = currentConfig[confNum];

	if(currentConfig == 0 || buzzer.type != buzzer_type)
		return;

	FILE *fp = fopen(buzzer.value_file, "w");
	fprintf(fp, "0\n");
	fclose(fp);
}

pthread_t beep(unsigned int confNum, int frequency, int duration)
{
	if(confNum > conf_size)
		return (pthread_t)-1;

	struct robo_dev buzzer = currentConfig[confNum];

	if(currentConfig == 0 || buzzer.type != buzzer_type)
		return (pthread_t)-1;

	FILE *fp = fopen(buzzer.value_file, "w");
	fprintf(fp, "%d\n", frequency);
	fclose(fp);

	return after_ms_do(duration, &stop_beep, confNum);
}

/////////////////////////////////////// Lifecycle /////////////////////////////////////

void __attribute__ ((constructor)) robo_initialize(void)
{
	currentConfig = (struct robo_dev*)0;
}

void __attribute__ ((destructor)) robo_release(void)
{
	if(currentConfig) {
		int i;
		for(i = 0; i < conf_size; i++) {
			free(currentConfig[i].value_file);
		}
		free(currentConfig);
	}
}
